﻿using System;
using System.Drawing;

namespace GeoTrans
{
    public struct Ellipsoid
    {
        public double SemiMajorAxis;
        public double SemiMinorAxis;
        public double E2;
        public double Ep2;
    }

    public struct Datum
    {
        public Ellipsoid Ellipsoid;

        //7 Params transformation from WGS84       
        public double Dx; //metros
        public double Dy; //metros
        public double Dz; //metros
        public double Rx; // rad
        public double Ry; //rad
        public double Rz; //rad
        public double p; //adimensional
    }

    public static class Converter
    {
        #region Geodetic Parameters


        public static readonly Ellipsoid WSG84 = new Ellipsoid()
        {
            SemiMajorAxis = 6378137.0,
            SemiMinorAxis = 6356752.314245,
            E2 = 0.006694379990197,
            Ep2 = 0.00673949674228
        };

        public static readonly Ellipsoid INT50 = new Ellipsoid()
        {
            SemiMajorAxis = 6378388.0,
            SemiMinorAxis = 6356911.9,
            E2 = 0.006722653187150,
            Ep2 = 0.006768153133383
        };

        public static readonly Datum DatumWSG84 = new Datum()
        {
            Ellipsoid = WSG84,
            Dx = 0,
            Dy = 0,
            Dz = 0,
            Rx = 0,
            Ry = 0,
            Rz = 0,
            p = 0
        };

        static readonly Datum DatumED50 = new Datum()
        {
            Ellipsoid = INT50,
            Dx = -102.0,
            Dy = -102.0,
            Dz = -129.0,
            Rx = 0.413 / SECONDS_PER_RADIAN,
            Ry = -0.184 / SECONDS_PER_RADIAN,
            Rz = 0.385 / SECONDS_PER_RADIAN,
            p = 0.0000024664
        };

        // Parameters for the GWS84 ellipsoid
        const double WGS84_E2 = 0.006694379990197;
        const double WGS84_E4 = WGS84_E2 * WGS84_E2;
        const double WGS84_E6 = WGS84_E4 * WGS84_E2;
        const double WGS84_SEMI_MAJOR_AXIS = 6378137.0; //metros
        const double WGS84_SEMI_MINOR_AXIS = 6356752.314245; //metros


        // Parameters for the Int ellipsoid        
        const double INT_SEMI_MAJOR_AXIS = 6378388.0;
        const double INT_SEMI_MINOR_AXIS = 6356912.0;
        const double INT_E2 = 0.006722653187150; //excentricidad 2
        const double INT_E4 = INT_E2 * INT_E2;
        const double INT_E6 = INT_E4 * INT_E2;
        const double INT_EP2 = 0.006768153133383; //segunda excentricidad 2

        // Parameters for UTM projection
        const double UTM_LONGITUDE_OF_ORIGIN = 3.0 / 180 * Math.PI;
        const double UTM_LATITUDE_OF_ORIGIN = 0;
        const double UTM_FALSE_EASTING = 500000;
        const double UTM_FALSE_NORTHING_N = 0;    // Northern hemisphere
        const double UTM_FALSE_NORTHING_S = 10000000; // Southern hemisphere
        const double UTM_SCALE_FACTOR = 0.9996;

        //7 Parameters for the GWS to EUS transformation
        const double SECONDS_PER_RADIAN = 206264.8062471;
        const double WGS_EUS_Dx = -102.0; //metros
        const double WGS_EUS_Dy = -102.0; //metros
        const double WGS_EUS_Dz = -129.0; //metros
        const double WGS_EUS_Rx = 0.413 / SECONDS_PER_RADIAN; // rad
        const double WGS_EUS_Ry = -0.184 / SECONDS_PER_RADIAN; //rad
        const double WGS_EUS_Rz = 0.385 / SECONDS_PER_RADIAN; //rad
        const double WGS_EUS_p = 0.0000024664; //adimensional

        #endregion

        /// <summary>
        /// Dadas las coordenadas WGS84 
        /// obtiene las UTM
        /// </summary>
        /// <param name="latitude"></param>
        /// <param name="longitude"></param>
        /// <param name="height"></param>
        /// <param name="easting"></param>
        /// <param name="northing"></param>
        public static void PolarWGS84ToUTMinED50(double latitude, double longitude, double height,
            out double easting, out double northing, out int zone)
        {
            //1 De polar a cartesiana en WGS 84
            double xw, yw, zw;
            GeodeticToGeocentricGWS84(latitude, longitude, height, out xw, out yw, out zw);

            //2 Cambio de datum de WGS84 a ED 50
            double xe, ye, ze;
            GeocentricDatumShiftFromWSG84toEUS(xw, yw, zw, out xe, out ye, out ze);

            //3 De cartesiana a polar en ED50
            double latED50, longED50, heightED50;
            Convert_Geocentric_To_Geodetic(xe, ye, ze, out latED50, out longED50, out heightED50);
            latED50 = latED50 * (360.0 / (2 * Math.PI));
            longED50 = longED50 * (360.0 / (2 * Math.PI));

            //4 Proyeccion UTM            
            DecDeg2UTM(latED50, longED50, out easting, out northing, out zone);
        }

        // Takes a lastGpsPosition in latitude / longitude as input
        // Returns lastGpsPosition in UTM easting/northing/zone (in meters)
        public static void DecDeg2UTM(double latitude, double longitude,
            out double easting, out double northing, out int zone)
        {
            // Normalize longitude into Zone, 6 degrees
            int int_zone = (int)(longitude / 6.0);
            if (longitude < 0)
                int_zone--;
            longitude -= (double)int_zone * 6.0;
            zone = int_zone + 31;    // UTM zone
            // Convert from decimal degrees to radians
            longitude *= Math.PI / 180.0;
            latitude *= Math.PI / 180.0;
            // Projection
            double M = INT_SEMI_MAJOR_AXIS * m_calc(latitude);
            double M_origin = INT_SEMI_MAJOR_AXIS * m_calc(UTM_LATITUDE_OF_ORIGIN);
            double A = (longitude - UTM_LONGITUDE_OF_ORIGIN) * Math.Cos(latitude);
            double A2 = A * A;
            double e2_prim = INT_E2 / (1 - INT_E2);
            double C = e2_prim * Math.Pow(Math.Cos(latitude), 2);
            double T = Math.Tan(latitude);
            T *= T;
            double v = INT_SEMI_MAJOR_AXIS / Math.Sqrt(1 - WGS84_E2 *
                Math.Pow(Math.Sin(latitude), 2));
            northing = UTM_SCALE_FACTOR * (M - M_origin + v * Math.Tan(latitude) * (
                A2 / 2 + (5 - T + 9 * C + 4 * C * C) * A2 * A2 / 24 +
                (61 - 58 * T + T * T + 600 * C - 330 * e2_prim) *
                A2 * A2 * A2 / 720));
            if (latitude < 0)
                northing += UTM_FALSE_NORTHING_S;
            easting = UTM_FALSE_EASTING + UTM_SCALE_FACTOR * v * (
                A + (1 - T + C) * A2 * A / 6 +
                (5 - 18 * T + T * T + 72 * C - 58 * e2_prim) * A2 * A2 * A / 120);

        }

        private static double m_calc(double lat)
        {
            return (1 - INT_E2 / 4 - 3 * INT_E4 / 64 - 5 * INT_E6 / 256) * lat -
                 (3 * INT_E2 / 8 + 3 * INT_E4 / 32 + 45 * INT_E6 / 1024) *
                 Math.Sin(2 * lat) + (15 * INT_E4 / 256 + 45 * INT_E6 / 1024) *
                 Math.Sin(4 * lat) - (35 * INT_E6 / 3072) * Math.Sin(6 * lat);
        }

        public static void GeodeticToGeocentricGWS84(double latitude, double longitude, double height,
            out double x, out double y, out double z)
        {
            latitude = latitude * (Math.PI / 180.0);//Radianes
            longitude = longitude * (Math.PI / 180.0);//Radianes

            double v = WGS84_SEMI_MAJOR_AXIS / (Math.Sqrt(1 - WGS84_E2 * Math.Sin(latitude) * Math.Sin(latitude)));

            //x=(ν+H)cosφc
            x = (v + height) * Math.Cos(latitude) * Math.Cos(longitude);
            //y=(ν+H)cosφsinλ
            y = (v + height) * Math.Cos(latitude) * Math.Sin(longitude);
            //z=((1−e2 )ν+H) sinφ
            z = ((1 - WGS84_E2) * v + height) * Math.Sin(latitude);

        }

        public static void GeodeticToGeocentric(Datum dt, double latitude, double longitude, double height,
            out double x, out double y, out double z)
        {
            latitude = latitude * (Math.PI / 180.0);//Radianes
            longitude = longitude * (Math.PI / 180.0);//Radianes

            double v = dt.Ellipsoid.SemiMajorAxis /
                (Math.Sqrt(1 - dt.Ellipsoid.E2 * Math.Sin(latitude) * Math.Sin(latitude)));

            //x=(ν+H)cosφc
            x = (v + height) * Math.Cos(latitude) * Math.Cos(longitude);
            //y=(ν+H)cosφsinλ
            y = (v + height) * Math.Cos(latitude) * Math.Sin(longitude);
            //z=((1−e2 )ν+H) sinφ
            z = ((1 - dt.Ellipsoid.E2) * v + height) * Math.Sin(latitude);
        }

        public static void GeocentricDatumShiftFromWSG84toEUS(double xo, double yo, double zo,
            out double xs, out double ys, out double zs)
        {
            xs = (xo - WGS_EUS_Dx) - WGS_EUS_Rz * yo + WGS_EUS_Ry * zo - WGS_EUS_p * xo;
            ys = (yo - WGS_EUS_Dy) + WGS_EUS_Rz * xo - WGS_EUS_Rx * zo - WGS_EUS_p * yo;
            zs = (zo - WGS_EUS_Dz) - WGS_EUS_Ry * xo + WGS_EUS_Rx * yo - WGS_EUS_p * zo;
        }

        public static void GeocentricDatumShiftFromWSG84(Datum dt, double xo, double yo, double zo,
            out double xs, out double ys, out double zs)
        {
            xs = (xo - dt.Dx) - dt.Rz * yo + dt.Ry * zo - dt.p * xo;
            ys = (yo - dt.Dy) + dt.Rz * xo - dt.Rx * zo - dt.p * yo;
            zs = (zo - dt.Dz) - dt.Ry * xo + dt.Rx * yo - dt.p * zo;
        }

        public static void Convert_Geocentric_To_Geodetic(double X, double Y, double Z,
                                     out double Latitude, out double Longitude, out double Height)
        { /*
             * The function Convert_Geocentric_To_Geodetic converts geocentric
             * coordinates (X, Y, Z) to geodetic coordinates (latitude, longitude, 
             * and height), according to the current ellipsoid parameters.
             *
             *    X         : Geocentric X coordinate, in meters.         (input)
             *    Y         : Geocentric Y coordinate, in meters.         (input)
             *    Z         : Geocentric Z coordinate, in meters.         (input)
             *    Latitude  : Calculated latitude value in radians.       (output)
             *    Longitude : Calculated longitude value in radians.      (output)
             *    Height    : Calculated height value, in meters.         (output)
             *
             * The method used here is derived from 'An Improved Algorithm for
             * Geocentric to Geodetic Coordinate Conversion', by Ralph Toms, Feb 1996
             */

            /* Note: Variable names follow the notation used in Toms, Feb 1996 */

            double W;        /* distance from Z axis */
            double W2;       /* square of distance from Z axis */
            double T0;       /* initial estimate of vertical component */
            double T1;       /* corrected estimate of vertical component */
            double S0;       /* initial estimate of horizontal component */
            double S1;       /* corrected estimate of horizontal component */
            double Sin_B0;   /* sin(B0), B0 is estimate of Bowring aux variable */
            double Sin3_B0;  /* cube of sin(B0) */
            double Cos_B0;   /* cos(B0) */
            double Sin_p1;   /* sin(phi1), phi1 is estimated latitude */
            double Cos_p1;   /* cos(phi1) */
            double Rn;       /* Earth radius at location */
            double Sum;      /* numerator of cos(phi1) */
            bool At_Pole;     /* indicates location is in polar region */
            double Geocent_b = INT_SEMI_MINOR_AXIS; /* Semi-minor axis of ellipsoid, in meters */

            //Constantes
            const double PI_OVER_2 = Math.PI / 2.0;
            const double COS_67P5 = 0.38268343236508977;  /* cosine of 67.5 degrees */
            const double AD_C = 1.0026000;
            const double Geocent_ep2 = 0.006768153133383;

            At_Pole = false;
            if (X != 0.0)
            {
                Longitude = Math.Atan2(Y, X);
            }
            else
            {
                if (Y > 0)
                {
                    Longitude = PI_OVER_2;
                }
                else if (Y < 0)
                {
                    Longitude = -PI_OVER_2;
                }
                else
                {
                    At_Pole = true;
                    Longitude = 0.0;
                    if (Z > 0.0)
                    {  /* north pole */
                        Latitude = PI_OVER_2;
                    }
                    else if (Z < 0.0)
                    {  /* south pole */
                        Latitude = -PI_OVER_2;
                    }
                    else
                    {  /* center of earth */
                        Latitude = PI_OVER_2;
                        Height = -Geocent_b;
                        return;
                    }
                }
            }
            W2 = X * X + Y * Y;
            W = Math.Sqrt(W2);
            T0 = Z * AD_C;
            S0 = Math.Sqrt(T0 * T0 + W2);
            Sin_B0 = T0 / S0;
            Cos_B0 = W / S0;
            Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
            T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
            Sum = W - INT_SEMI_MAJOR_AXIS * INT_E2 * Cos_B0 * Cos_B0 * Cos_B0;
            S1 = Math.Sqrt(T1 * T1 + Sum * Sum);
            Sin_p1 = T1 / S1;
            Cos_p1 = Sum / S1;
            Rn = INT_SEMI_MAJOR_AXIS / Math.Sqrt(1.0 - INT_E2 * Sin_p1 * Sin_p1);
            if (Cos_p1 >= COS_67P5)
            {
                Height = W / Cos_p1 - Rn;
            }
            else if (Cos_p1 <= -COS_67P5)
            {
                Height = W / -Cos_p1 - Rn;
            }
            else
            {
                Height = Z / Sin_p1 + Rn * (INT_E2 - 1.0);
            }
            if (At_Pole == false)
            {
                Latitude = Math.Atan(Sin_p1 / Cos_p1);
            }
            else
            {
                Latitude = 0.0;
            }
        }

        public static void Convert_Geocentric_To_Geodetic(Datum dt, double X, double Y, double Z,
                                     out double Latitude, out double Longitude, out double Height)
        {
            /*
            * The function Convert_Geocentric_To_Geodetic converts geocentric
            * coordinates (X, Y, Z) to geodetic coordinates (latitude, longitude, 
            * and height), according to the current ellipsoid parameters.
            *
            *    X         : Geocentric X coordinate, in meters.         (input)
            *    Y         : Geocentric Y coordinate, in meters.         (input)
            *    Z         : Geocentric Z coordinate, in meters.         (input)
            *    Latitude  : Calculated latitude value in radians.       (output)
            *    Longitude : Calculated longitude value in radians.      (output)
            *    Height    : Calculated height value, in meters.         (output)
            *
            * The method used here is derived from 'An Improved Algorithm for
            * Geocentric to Geodetic Coordinate Conversion', by Ralph Toms, Feb 1996
            */

            /* Note: Variable names follow the notation used in Toms, Feb 1996 */

            double W;        /* distance from Z axis */
            double W2;       /* square of distance from Z axis */
            double T0;       /* initial estimate of vertical component */
            double T1;       /* corrected estimate of vertical component */
            double S0;       /* initial estimate of horizontal component */
            double S1;       /* corrected estimate of horizontal component */
            double Sin_B0;   /* sin(B0), B0 is estimate of Bowring aux variable */
            double Sin3_B0;  /* cube of sin(B0) */
            double Cos_B0;   /* cos(B0) */
            double Sin_p1;   /* sin(phi1), phi1 is estimated latitude */
            double Cos_p1;   /* cos(phi1) */
            double Rn;       /* Earth radius at location */
            double Sum;      /* numerator of cos(phi1) */
            bool At_Pole;     /* indicates location is in polar region */
            double Geocent_b = dt.Ellipsoid.SemiMinorAxis; /* Semi-minor axis of ellipsoid, in meters */
            double Geocent_ep2 = dt.Ellipsoid.Ep2;

            //Constantes
            const double PI_OVER_2 = Math.PI / 2.0;
            const double COS_67P5 = 0.38268343236508977;  /* cosine of 67.5 degrees */
            const double AD_C = 1.0026000;


            At_Pole = false;
            if (X != 0.0)
            {
                Longitude = Math.Atan2(Y, X);
            }
            else
            {
                if (Y > 0)
                {
                    Longitude = PI_OVER_2;
                }
                else if (Y < 0)
                {
                    Longitude = -PI_OVER_2;
                }
                else
                {
                    At_Pole = true;
                    Longitude = 0.0;
                    if (Z > 0.0)
                    {  /* north pole */
                        Latitude = PI_OVER_2;
                    }
                    else if (Z < 0.0)
                    {  /* south pole */
                        Latitude = -PI_OVER_2;
                    }
                    else
                    {  /* center of earth */
                        Latitude = PI_OVER_2;
                        Height = -Geocent_b;
                        return;
                    }
                }
            }

            W2 = X * X + Y * Y;
            W = Math.Sqrt(W2);
            T0 = Z * AD_C;
            S0 = Math.Sqrt(T0 * T0 + W2);
            Sin_B0 = T0 / S0;
            Cos_B0 = W / S0;
            Sin3_B0 = Sin_B0 * Sin_B0 * Sin_B0;
            T1 = Z + Geocent_b * Geocent_ep2 * Sin3_B0;
            Sum = W - dt.Ellipsoid.SemiMajorAxis * dt.Ellipsoid.E2 * Cos_B0 * Cos_B0 * Cos_B0;
            S1 = Math.Sqrt(T1 * T1 + Sum * Sum);
            Sin_p1 = T1 / S1;
            Cos_p1 = Sum / S1;
            Rn = dt.Ellipsoid.SemiMajorAxis / Math.Sqrt(1.0 - dt.Ellipsoid.E2 * Sin_p1 * Sin_p1);

            if (Cos_p1 >= COS_67P5)
            {
                Height = W / Cos_p1 - Rn;
            }
            else if (Cos_p1 <= -COS_67P5)
            {
                Height = W / -Cos_p1 - Rn;
            }
            else
            {
                Height = Z / Sin_p1 + Rn * (dt.Ellipsoid.E2 - 1.0);
            }

            if (At_Pole == false)
            {
                Latitude = Math.Atan(Sin_p1 / Cos_p1);
            }
            else
            {
                Latitude = 0.0;
            }
        }

        #region IGN25

        public static Point UtmToPx(Point utmPoint)
        {
            Point pxPoint = new Point();
            pxPoint.X = (utmPoint.X - 109) / 4;
            pxPoint.Y = (utmPoint.Y - 1229) / 4;
            return pxPoint;
        }

        public static Point PxToUtm(Point pxPoint)
        {
            Point utmPoint = new Point();
            utmPoint.X = 4 * pxPoint.X + 108;
            utmPoint.Y = 4 * pxPoint.Y + 1228;
            return utmPoint;
        }

        public static Point UtmToTile(Point utmPoint)
        {
            Point pxPoint = UtmToPx(utmPoint);
            Point tilePoint = new Point();
            tilePoint.X = pxPoint.X / 256;
            tilePoint.Y = (pxPoint.Y / 256) + 1;
            return tilePoint;
        }

        public static Point TileToPx(Point tilePoint)
        {
            return new Point(tilePoint.X * 256, tilePoint.Y * 256);
        }

        #endregion

    }
}
